﻿using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;
using System.Drawing;
using Cambia.CoreLib;

namespace LocalizationTest
{
  public class ContinuousActionPose2D : ActionDescription
  {
    public ContinuousActionPose2D()
    {
      action = new Pose2D();
    }
    public ContinuousActionPose2D(Pose2D action)
    {
      this.action = action;
    }
    public Pose2D action;
    public override ActionDescription InvertAction()
    {
//      return new ContinuousActionPose2D(new Pose2D(-action.translation.x, -action.translation.y, -action.rotation));
      return new ContinuousActionPose2D(new Pose2D().minusDiff(this.action));
    }

    public override WorldState Execute(MotionModel motionModel, WorldState oldState)
    {
      if (Globals.selfLocatorProperties.MotionNoise == NoiseType.noNoise)
        return ExecutePerfect(motionModel, oldState);
      else
        return ExecuteWithError(motionModel, oldState);
    }

    public override WorldState ExecutePerfect(MotionModel motionModel, WorldState oldState)
    {
      /*
      double newPositionX = ((WorldStatePose2D)oldState).Pose.translation.x;
      double newPositionY = ((WorldStatePose2D)oldState).Pose.translation.y;
      double newRotation = ((WorldStatePose2D)oldState).Pose.rotation;

      return new WorldStatePose2D(
        new Pose2D(
        newPositionX + this.action.translation.x, 
        newPositionY + this.action.translation.y, 
        newRotation + this.action.rotation));*/
      return new WorldStatePose2D( ((WorldStatePose2D)oldState).Pose + this.action );
    }

    private WorldState ExecuteWithError(MotionModel motionModel, WorldState oldState)
    {
      double systematicErrorX = this.action.translation.x / 10.0;
      double systematicErrorY = this.action.translation.y / 10.0;
      double systematicErrorRotation = this.action.rotation / 10.0;

      double gaussianErrorX = RandomProvider.NextNormal() * this.action.translation.x / 10.0; //sigma; // Math.Sqrt(sigma);
      double gaussianErrorY = RandomProvider.NextNormal() * this.action.translation.y / 10.0; // Math.Sqrt(sigma);
      double gaussianErrorRotation = RandomProvider.NextNormal() * this.action.rotation / 10.0; // Math.Sqrt(sigma);

      if (Globals.selfLocatorProperties.MotionNoise == NoiseType.gaussianNoSystematic)
      {
        systematicErrorX = 0;
        systematicErrorY = 0;
        systematicErrorRotation = 0;
      }
      if (Globals.selfLocatorProperties.MotionNoise == NoiseType.noGaussianButSystematic)
      {
        gaussianErrorX = 0;
        gaussianErrorY = 0;
        gaussianErrorRotation = 0;
      }

      Pose2D actionWithError = new Pose2D(
        this.action.translation.x + gaussianErrorX + systematicErrorX,
        this.action.translation.y + gaussianErrorY + systematicErrorY,
        this.action.rotation + gaussianErrorRotation + systematicErrorRotation );

      Pose2D newPose = ((WorldStatePose2D)oldState).Pose + actionWithError;

      Random random = new Random();
      if (random.NextDouble() < 0.06)
      {
        newPose.translation.x += RandomProvider.NextNormal() * 20.0;
        newPose.translation.y += RandomProvider.NextNormal() * 20.0;
        newPose.rotation += RandomProvider.NextNormal() * 0.4;
      }

      return new WorldStatePose2D(newPose);
    }

    public override string ToString()
    {
      return 
        this.GetType().Name + "|" + 
        action.ToString();
    }

    public override ActionDescription Parse(string str)
    {
      //string[] parts = str.Split('|');
      return new ContinuousActionPose2D(Pose2D.Parse(str) );
    }

  }
/*
  public class ContinuousActionPose2DCreator
  {
    public Vector2 target;

    public double currentSpeed; //mm per frame
    public double currentRotationSpeed; //rad per frame

    double maxSpeed = 15;

    private Pose2D currentPose;

    public ContinuousActionPose2DCreator()
    {
      Reset(new Pose2D());
    }

    public ContinuousActionPose2DCreator(Pose2D initialPose)
    {
      Reset(initialPose);
    }

    public void Reset(Pose2D initialPose)
    {
      currentSpeed = 0;
      currentRotationSpeed = 0;
      currentPose = initialPose;
      target = new Vector2();
    }

    public Vector2 GetPositionDifference(WorldStatePose2D simulatedWorldState)
    {
      currentPose.translation.x = simulatedWorldState.Vector.x;
      currentPose.translation.y = simulatedWorldState.Vector.y;
      currentRotationSpeed = Geometry.AngleTo(currentPose, target) / 2.0;

      double distanceToTarget = Geometry.DistanceTo(currentPose, target);
      if (distanceToTarget > 100 && currentSpeed < maxSpeed) currentSpeed += 1;
      if (distanceToTarget < maxSpeed * 2 ) currentSpeed = distanceToTarget / 2;

      DebugDrawingTool.CreateArrow("world:action creator:speed",
        currentPose.translation,
        currentPose.translation + new Vector2(Math.Cos(currentPose.rotation) * currentSpeed, Math.Sin(currentPose.rotation) * currentSpeed),
        4, Color.Red);

      Pose2D relativeChange = GetRelativePoseChangePerFrame();
      Pose2D oldPose = currentPose;
      currentPose += relativeChange;
      return currentPose.translation - oldPose.translation;
    }

    private Pose2D GetRelativePoseChangePerFrame()
    {
      if (currentRotationSpeed == 0)
      {
        return new Pose2D(currentSpeed, 0, 0);
      }
      else
      {
        double numberOfFramesNeededForFullTurn = Math.PI * 2.0 / currentRotationSpeed;
        double circumferenceOfCircle = currentSpeed * numberOfFramesNeededForFullTurn;
        double radiusOfCirlce = circumferenceOfCircle / (Math.PI * 2.0);
        Pose2D start = new Pose2D();
        Pose2D end = new Pose2D(
          Math.Sin(currentRotationSpeed) * radiusOfCirlce,
          radiusOfCirlce - Math.Cos(currentRotationSpeed) * radiusOfCirlce,
          currentRotationSpeed);
        return end - start;
      }
    }
  }


  public class ContinuousActionPose2DCreatorOld
  {
    public double currentSpeed; //mm per frame
    public double currentRotationSpeed; //rad per frame

    private Pose2D currentPose;

    public ContinuousActionPose2DCreatorOld(Pose2D initialPose)
    {
      Reset(initialPose);
    }

    public ContinuousActionPose2DCreatorOld()
    {
      Reset(new Pose2D());
    }

    public void Reset(Pose2D initialPose)
    {
      currentSpeed = 0;
      currentRotationSpeed = 0;
      currentPose = initialPose;
    }
    
    public Vector2 GetPositionDifference()
    {
      DebugDrawingTool.CreateArrow("world:action creator:speed",
        currentPose.translation,
        currentPose.translation + new Vector2(Math.Cos(currentPose.rotation) * currentSpeed, Math.Sin(currentPose.rotation) * currentSpeed),
        4, Color.Red);

      Pose2D relativeChange = GetRelativePoseChangePerFrame();
      Pose2D oldPose = currentPose;
      currentPose += relativeChange;
      return currentPose.translation - oldPose.translation;
    }

    private Pose2D GetRelativePoseChangePerFrame()
    {
      if (currentRotationSpeed == 0)
      {
        return new Pose2D(currentSpeed, 0, 0);
      }
      else
      {
        double numberOfFramesNeededForFullTurn = Math.PI * 2.0 / currentRotationSpeed;
        double circumferenceOfCircle = currentSpeed * numberOfFramesNeededForFullTurn;
        double radiusOfCirlce = circumferenceOfCircle / (Math.PI * 2.0);
        Pose2D start = new Pose2D();
        Pose2D end = new Pose2D(
          Math.Sin(currentRotationSpeed) * radiusOfCirlce,
          radiusOfCirlce - Math.Cos(currentRotationSpeed) * radiusOfCirlce,
          currentRotationSpeed);
        return end - start;
      }
    }
  }
    */
}
